"""
#===============================================================================
# License
#===============================================================================
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

#===============================================================================
Copyright (c) 2009 bryan p. conrad
bconrad2@gmail.com

Repository Path:    
$HeadURL$

Created on:           Oct 1, 2009
Last Revised on:      $Date$
Version:              $Revision$
ID:                   $Id$
Author:               $Author$ :: (bryan)

#===============================================================================


#===============================================================================
# Documentation
#===============================================================================

"""

#===============================================================================
# Imports
#===============================================================================
import numpy as np
from scipy import interpolate
import vtk

import pylab
import JT
import os
import cPickle as pickle
#import spin_calc
from scipy import stats 

try:
    import sympy
except:
    print "Cannot import sympy, no symbolic math"

try:
    from marker_data_read import marker_data_read
except:
    print "Can not find marker_data_read module"
    
def get_joint_pose(pose1, pose2, seq='213'):
    """
    calculate angles and displacements between two bodies.
    pose is in the form X-trans,Y-trans,Z-trans,X-rot,Y-rot-Z-rot.
    rotations are in radians!. seq is the euler sequence.
    
    Returns the pose of body 2 wrt body 1
    """
    # Make sure pose is an array
    pose1 = np.asarray(pose1)
    pose2 = np.asarray(pose2)
    if len(pose2.shape)!=len(pose1.shape):
            # there is a problem
            print "Error. Pose1 and Pose2 are not the same length."
            return
    if len(pose1.shape)==1:
        # one dimensional array    
        # Transformation matrix for body 1
        T1 = get_transform2(pose1,seq)
        T2 = get_transform2(pose2,seq)
        T12 = np.dot(np.linalg.inv(T1),T2)
        return JT.Kinematics.get_pose(T12,seq)
    else:
        # pose is 2 dimensional array
        out_pose = []
        n_frames = pose1.shape[0]
        for f in range(n_frames):
            T1 = get_transform2(pose1[f,:],seq)
            T2 = get_transform2(pose2[f,:],seq)
            T12 = np.dot(np.linalg.inv(T1),T2)

            out_pose.append(JT.Kinematics.get_pose(T12,seq))
        out_pose = np.asarray(out_pose)
        return out_pose


def plot_results_from_project_file(project_file,choice=None):
    
    data = JT.Data2.DataStructure()
    data.load(project_file)
    volume_paths = data.get_unique_path_list('Volume')
    for i,path in enumerate(volume_paths):
        print "%i - %s" % (i,path)
    if choice is None:
        choice = raw_input("Joint angles between which volumes? (eg. 12 or 32): ")
    #choice = '01'
    if len(choice) != 2:
        print "Error: You must input a two digit number"
    
    poses1 = data.get_all_poses(volume_paths[int(choice[0])])
    poses2 = data.get_all_poses(volume_paths[int(choice[1])]) 
    plot_results(poses1,poses2)

def plot_results_from_string(xml_string,choice=None):
    """
    xml string
    """
    data = JT.Data2.DataStructure()
    data.load_string(xml_string)
    volume_paths = data.get_unique_path_list('Volume')
    for i,path in enumerate(volume_paths):
        print "%i - %s" % (i,path)
    if choice is None:
        choice = raw_input("Joint angles between which volumes? (eg. 12 or 32): ")
    #choice = '01'
    if len(choice) != 2:
        print "Error: You must input a two digit number"
        
    poses1 = data.get_all_poses(volume_paths[int(choice[0])])
    poses2 = data.get_all_poses(volume_paths[int(choice[1])]) 
    plot_results(poses1,poses2)
        
def plot_results(poses1,poses2):
    
    
#    fig = pylab.figure()
#    ax1 = fig.add_subplot(211)
#    ax1.plot(poses1[:,0,5]*180/np.pi)
#    ax1.set_title('Model 1')
#    ax2 = fig.add_subplot(212)
#    ax2.plot(poses2[:,0,5]*180/np.pi)
#    ax2.set_title('Model 2')
#    pylab.show()
    
    if poses1.shape != poses2.shape:
        print "Error: poses are not the same length"
        return
    
    joint_poses = []
    for i in range(len(poses1)):
        joint_poses.append(get_joint_pose(poses1[i,0,:],poses2[i,0,:]))
    
    joint_poses = np.asarray(joint_poses)
    joint_poses[:,3:6] = joint_poses[:,3:6]*180/np.pi
    print "Joint poses shape: %s" % str(joint_poses.shape)
    
    fig = pylab.figure()
    for i in range(6):
        ax = fig.add_subplot(6,1,i+1)
        ax.plot(joint_poses[:,i])
    
    pylab.show()

def plot_3x3(data,x_attrib,y_attrib,y_range=(-30,30),bone=0,tag='',rad=False):
    """
    This function will plot a 3x3 grid of charts for the each dimension of 
    motion in a row and each trial in a column.
     
    Data is displacements and rotations over time in the format:
        [Specimen, Test, bone, Frame, Dimension]
        Dimension is a 6 element vector with translations then rotations (radians).
    attrib is the attribute that you would like to plot: 
        [mocap_results,jt_results,errors]
    ref_angle is an array of values that is the same length as data and will
        be used as the x-data in the plots.  If ref_angles is not given then 
        the plot x-axis will be frame number.
    bone specifies which bone should be plotted (index) defaults to first
    tag is a label that will be used to give the figure a name
    rad is a bool that indicates whether the data is supplied in radians.  
        data will be plotted in degrees, and will be converted if 
        necessary (rad=True)
    
    This function will return a list of two pylab figure objects [trans,rots], 
    which can then be saved or displayed to the screen.
    """
    
    labels = ['ML Trans(mm)','AP Trans (mm)','SI Trans (mm)',
              'FE (deg)', 'LB (deg)', 'AR (deg)']
    titles = ['Trial1\nFlexion-Extension',
              'Trial2\nLateral Bending',
              'Trial3\nAxial Rotation']
    
    y_min_trans = np.ones(3)*1e30
    y_max_trans = np.ones(3)*-1e30
    y_min_angle = np.ones(3)*1e30
    y_max_angle = np.ones(3)*-1e30
    
    x_min = np.ones(3)*1e30
    x_max = np.ones(3)*-1e30
    #x_min_angle = np.ones(3)*1e30
    #x_max_angle = np.ones(3)*-1e30
    
    # remove nans from array
    #data[np.isnan(data)]=0
    fig_trans = pylab.figure()
    fig_trans.hold(True)
    fig_trans.suptitle(tag+' Segmental Translation',fontsize=16)
    fig_trans.canvas.set_window_title('Translation')
    fig_angle = pylab.figure()
    fig_angle.hold(True)
    fig_angle.suptitle(tag+' Segmental Rotation',fontsize=16)
    fig_angle.canvas.set_window_title('Rotation')
    
    for i in range(3):
        # iterate over each spine
        fig_angle.subplots_adjust(wspace=.1,hspace=.1,top=.8,right=.8)
        fig_trans.set_facecolor('white')
        fig_trans.subplots_adjust(wspace=.1,hspace=.1,top=.8,right=.8)
        fig_angle.set_facecolor('white')
        for j in range(3):
            # iterate over each motion direction
            for k in range(3):
                # iterate over each joint angle dimension
                # data format [Specimen,test dir,frame,dim]
                if bone is None:
                    trans = data[i].__getattribute__(y_attrib)[k][:,j]
                    if rad==False:
                        angle = data[i].__getattribute__(y_attrib)[k][:,j+3] #*180/np.pi
                    else:
                        angle = data[i].__getattribute__(y_attrib)[k][:,j+3]*180/np.pi
                    trans[np.isnan(trans)]=0
                    angle[np.isnan(angle)]=0
                else:
                    if type(data)==type([]):
                        # if data is a list
                        trans = data[i].__getattribute__(y_attrib)[k][bone][:,j]
                        if rad==False:
                            angle = data[i].__getattribute__(y_attrib)[k][bone][:,j+3]#*180/np.pi
                        else:
                            angle = data[i].__getattribute__(y_attrib)[k][bone][:,j+3]*180/np.pi
                        trans[np.isnan(trans)]=0
                        angle[np.isnan(angle)]=0
                    else:
                        trans = data[i,k,bone,:,j]
                        if rad==False:
                            angle = data[i,k,bone,:,j+3]#*180/np.pi
                        else:
                            angle = data[i,k,bone,:,j+3]*180/np.pi
                    
                # Find charting limits of y data
                if trans.min()<y_min_trans[j]: y_min_trans[j] = trans.min()
                if trans.max()>y_max_trans[j]: y_max_trans[j] = trans.max()
                if angle.min()<y_min_angle[j]: y_min_angle[j] = angle.min()
                if angle.max()>y_max_angle[j]: y_max_angle[j] = angle.max()
                
                #print "Plotting spine %i, Trial %i, dimension %i" % (i,j,k)
                ax_trans = fig_trans.add_subplot(3,3,3*j+k+1,axisbg='1')
                ax_angle = fig_angle.add_subplot(3,3,3*j+k+1,axisbg='.9')
#               
                if x_attrib.lower() == 'frames':
                    # plot frames on x-axis, just a range of length to match
                    # angle and trans
                    x_data = range(len(angle))
                    ax_trans.plot(trans,linewidth=1,
                                  label='Spine'+str(i+1))
                    ax_angle.plot(angle,linewidth=1,
                                  label='Spine'+str(i+1))
                    x_min[k] = 0
                    x_max[k] = len(x_data)
                else:
                    # x data is an angle
                    # x_data will be the same for all dims, it is indexed to trial
                    if rad==False:
                        x_data = data[i].__getattribute__(x_attrib)[k][bone][:,k+3]
                    else:
                        x_data = data[i].__getattribute__(x_attrib)[k][bone][:,k+3]*180/np.pi
                    #print "x_data %s, trans %s" % (str(x_data.shape),str(trans.shape))
                    if (len(x_data) > len(trans)):
                        # if x_data is longer, add zeros to the end of trans and angle
                        new_trans = np.zeros(x_data.shape)
                        new_trans[0:len(trans)] = trans
                        trans = new_trans
                        new_angle = np.zeros(x_data.shape)
                        new_angle[0:len(angle)] = angle
                        angle = new_angle
                    if (len(x_data) < len(trans)):
                        # if trans is longer, add zeros to the end of x_data
                        new_x_data = np.zeros(trans.shape)
                        new_x_data[0:len(x_data)] = x_data
                        x_data = new_x_data
                        
                    ax_trans.plot(x_data,trans,linewidth=1,
                                  label='Spine'+str(i+1))
                    ax_angle.plot(x_data,angle,linewidth=1,
                                  label='Spine'+str(i+1))
                    # Find charting limits of x data
                    if x_data.min()<x_min[k]: x_min[k] = x_data.min()
                    if x_data.max()>x_max[k]: x_max[k] = x_data.max()
                    
                
                ax_trans.set_xticks([])
                ax_trans.set_yticks([])
                ax_angle.set_xticks([])
                ax_angle.set_yticks([])
                
                if y_range == 0:
                    ax_trans.set_ylim([int(y_min_trans[j])-1,int(y_max_trans[j])+1])
                    ax_angle.set_ylim([int(y_min_angle[j])-1,int(y_max_angle[j])+1])
                elif y_range == 'auto':
                    ax_trans.set_ylim([int(y_min_trans[j])-1,int(y_max_trans[j])+1])
                    ax_angle.set_ylim([int(y_min_angle[j])-1,int(y_max_angle[j])+1])
                else:
                    ax_trans.set_ylim(y_range)
                    ax_angle.set_ylim(y_range)
                ax_trans.set_xlim([int(x_min[k])-1,int(x_max[k])+1])
                ax_angle.set_xlim([int(x_min[k])-1,int(x_max[k])+1])
                
                if k == j:
                    # Diagonal Charts
                    # Make diagonal charts white, off diagonal grey (0.9)
                    ax_angle.set_axis_bgcolor('1')
                if k == 0:
                    # First column, Draw axis labels and ticks
                    ax_trans.set_ylabel(labels[j])
                    ax_angle.set_ylabel(labels[j+3])
                    if y_range == 0:
                        ax_trans.set_yticks([int(y_min_trans[j])-1,int(y_max_trans[j])+1])
                        ax_angle.set_yticks([int(y_min_angle[j])-1,int(y_max_angle[j])+1])
                    if y_range == 'auto':
                        ax_trans.set_yticks([int(y_min_trans[j])-1,int(y_max_trans[j])+1])
                        ax_angle.set_yticks([int(y_min_angle[j])-1,int(y_max_angle[j])+1])
                    else:
                        ax_trans.set_yticks(y_range)
                        ax_angle.set_yticks(y_range)
                if j == 0:
                    # First Row, Print column titles
                    ax_trans.set_title(titles[k])
                    ax_angle.set_title(titles[k])
                    if k == 2:
                        # This should place the legend outside the chart area
                        ax_trans.legend(loc=(1.1,.35))
                        ax_angle.legend(loc=(1.1,.35))
                if j == 2:
                    # Last Row, set x tick
                    if x_attrib.lower() == 'frames':
                        ax_trans.set_xticks([len(trans)/2,len(trans)])
                        ax_angle.set_xticks([len(angle)/2,len(angle)])  
                        ax_trans.set_xlabel('Frames')
                        ax_angle.set_xlabel('Frames')  
                    else:
                        ax_trans.set_xticks([int(x_min[k]),int(x_max[k])])
                        ax_angle.set_xticks([int(x_min[k]),int(x_max[k])])
                        #ax_trans.set_xticks([int(x_data.min()),int(x_data.max())])
                        #ax_angle.set_xticks([int(x_data.min()),int(x_data.max())])  
                        ax_trans.set_xlabel('Angle')
                        ax_angle.set_xlabel('Angle')  
                         
                    
    return [fig_trans,fig_angle]


def interpolate_poses(poses,rate1,rate2):
    """
    This function will interpolate the given poses collected at rate1 to 
    match rate2.  Poses are as returned from JT.DataStructure(), with 
    format [frame,volume,dimension(translations, rotations)]
    Returns a numpy array of interpolated poses
    """
  
    x = np.linspace(0,len(poses)/rate1,len(poses))
    newx = np.linspace(0,(len(poses))/rate1,(len(poses))*(rate2/rate1))   
    poses_interp = np.zeros((len(newx),poses.shape[1],6))
    for i in range(poses.shape[1]):
        # iterate through each volume
        for j in range(6):
            # and through each dimension
            #cj = cspline1d(poses[:,vol,i])
            f = interpolate.interp1d(x, poses[:,i,j])
            poses_interp[:,i,j] = f(newx)

    return poses_interp


def jt_anatomic_joint_angles(spine_data,joint='01',save_data=True,plot=True):
    """
    This function finds the joint angles between two vertebrae from pyTrack 
    data.
    Inputs:     - data is a class with attributes for the parameters below
                - anatomical landmarks in CT coordinates
                - origin (center of rotation for pyTrack) 
                    of volume in CT coordinates (from project file) 
                - pyTrack project which has registered poses of both volumes
                - data_config_file is a python module that contains the individual
                    data for each specimen
                - joint is a two digit string that identifies which volume 
                    indices that form the joint that is being evaluated.  
                    For example, if the joint is the second volume wrt the first, 
                    the string '01' would be given.  If the second volume wrt the
                    third volume then '12' would be used.
                - [optional] a filename to save the results to
    
    """
    if len(joint)!=2:
        print "The string 'joint' must be a string of 2 integer characters"
        return
    
    body1 = int(joint[0])
    body2 = int(joint[1])
    # Anatomic Landmarks in CT coordinates
    # Test marker coords from ImageJ
    ant_pos_1 = spine_data.landmark_list[body1][0]
    left_pos_1 = spine_data.landmark_list[body1][1]
    right_pos_1 = spine_data.landmark_list[body1][2]
    
    # Anatomic Landmarks in CT coordinates
    # Test marker coords from ImageJ
    ant_pos_2 = spine_data.landmark_list[body2][0]
    left_pos_2 = spine_data.landmark_list[body2][1]
    right_pos_2 = spine_data.landmark_list[body2][2]
#===============================================================================
#    CT
#===============================================================================
    # Transform from volume CT coords to anatomical coor sys
    # based on landmarks from CT images
    T_v1_anat = build_local(left_pos_1,right_pos_1,ant_pos_1)
    T_v2_anat = build_local(left_pos_2,right_pos_2,ant_pos_2)
    
#===============================================================================
#    Flouro (pyTrack)
#===============================================================================
    all_joint_angles = []
    all_vert1_angles = []
    all_vert2_angles = []
    # Load data from each xml project file in the spine data class
    project_list = spine_data.jt_project_list
    for project_file in project_list:
        jt_data = JT.Data2.DataStructure()
        jt_data.load(project_file)
        poses = jt_data.get_all_poses()
        
        # Interpolate poses to match mocap data collection rate
        # Flouroscopic data collection rate
        fl_rate = 7.5
        mocap_rate = 60
        x = np.linspace(0,len(poses)/fl_rate,len(poses))
        newx = np.linspace(0,(len(poses))/fl_rate,(len(poses))*(mocap_rate/fl_rate))   
        poses_interp = np.zeros((len(newx),poses.shape[1],6))
        for vol in range(poses.shape[1]):
            # iterate through each volume
            for i in range(6):
                # and through each dimension
                #cj = cspline1d(poses[:,vol,i])
                f = interpolate.interp1d(x, poses[:,vol,i])
                poses_interp[:,vol,i] = f(newx)
        
        print jt_data.get_unique_path_list('Volume')
        
    #    pylab.plot(poses_interp[:,:,5])
    #    pylab.show()
        
        # Get Poses from data structure
        vol_paths = jt_data.get_unique_path_list('Volume')
        img_path = jt_data.get_current_image_path()
        
        vol1_poses = poses_interp[:,0,:]
        vol2_poses = poses_interp[:,1,:]
        
        # Get origin of volumes (center) from pyTrack data file
        center1 = jt_data.get_center(img_path, vol_paths[0])
        center2 = jt_data.get_center(img_path, vol_paths[1])
        T_v1_v01 = get_transform2((center1[0],center1[1],center1[2],0,0,0),seq='213')
        T_v2_v02 = get_transform2((center2[0],center2[1],center2[2],0,0,0),seq='213')  
        
        # Calculate second pose transformation  (first might be bad)
        T_f_v01_1 = JT.Kinematics.get_transform2(vol1_poses[1,:], '213')
        # Calculate Static Transformation 
        T_static1 = np.dot(T_f_v01_1,np.dot(np.linalg.inv(T_v1_v01),T_v1_anat))
        T_f_v02_1 = JT.Kinematics.get_transform2(vol2_poses[1,:], '213')
        # Calculate Static Transformation 
        T_static2 = np.dot(T_f_v02_1,np.dot(np.linalg.inv(T_v2_v02),T_v2_anat))
        
        # ***DUBUG****
        #pylab.plot((vol1_poses[:,5]-vol2_poses[:,5])*180/np.pi)
        #pylab.show()
        # ***DUBUG****
        
        # iterate through each frame in the poses.
        joint_angles = []
        vert1_angles = []
        vert2_angles = []
        n_frames = len(vol1_poses)
        for i in range(n_frames):
            vol1_pose = vol1_poses[i,:]
            vol2_pose = vol2_poses[i,:]
        
            # Transforms from flouro to volumes (from pyTrack)
            T_fl_v01 = get_transform2(vol1_pose,seq='213')
            T_fl_v02 = get_transform2(vol2_pose,seq='213')
            
            T_fl_anat_v1 = np.dot(T_fl_v01,np.dot(np.linalg.inv(T_v1_v01),T_v1_anat))
            T_fl_anat_v2 = np.dot(T_fl_v02,np.dot(np.linalg.inv(T_v2_v02),T_v2_anat))
            
            T = np.dot(np.linalg.inv(T_fl_anat_v1),T_fl_anat_v2)
            joint_angles.append(get_pose(T,'213'))
            
            # Calculate individual vertebrae transfomations
            T_motion1 = np.dot(T_fl_v01,np.dot(np.linalg.inv(T_v1_v01),T_v1_anat))
            T_static_motion1 = np.dot(np.linalg.inv(T_static1),T_motion1)
            # Find euler angles using 213 [VTK] sequence
            vert1_angles.append(JT.Kinematics.get_pose(T_static_motion1, '213'))
            T_motion2 = np.dot(T_fl_v02,np.dot(np.linalg.inv(T_v2_v02),T_v2_anat))
            T_static_motion2 = np.dot(np.linalg.inv(T_static2),T_motion2)
            # Find euler angles using 213 [VTK] sequence
            vert2_angles.append(JT.Kinematics.get_pose(T_static_motion2, '213'))
            
        
        joint_angles = np.asarray(joint_angles)
        vert1_angles = np.asarray(vert1_angles)
        vert2_angles = np.asarray(vert2_angles)
        
        all_joint_angles.append(joint_angles)
        all_vert1_angles.append(vert1_angles)
        all_vert2_angles.append(vert2_angles)
        
        if plot:
            plot_joint_angles(joint_angles)
        if save_data == True:
            print "Cannot save data in current format"
#            save_name_0 = project_file.replace('.xml','_'+joint+'.dat')
#            save_joint_angles(save_name_0, all_joint_angles)
#            save_name_1 = project_file.replace('.xml','_vert1'+'.dat')
#            save_joint_angles(save_name_1, all_vert1_angles)
#            save_name_2 = project_file.replace('.xml','_vert2'+'.dat')
#            save_joint_angles(save_name_2, all_vert2_angles)
#            print "Data saved to %s" % save_name_0
#            print "Data saved to %s" % save_name_1
#            print "Data saved to %s" % save_name_2
    
    # Construct an array for all the output pose data
    nmax = 0
    for angles in all_joint_angles:        
        n = len(angles)
        if n>nmax: nmax=n
    output_angles = np.zeros((len(all_joint_angles),nmax,6))
    output_vert1 = np.zeros((len(all_vert1_angles),nmax,6))
    output_vert2 = np.zeros((len(all_vert1_angles),nmax,6))
    for i,angles in enumerate(all_joint_angles):
        output_angles[i,0:len(angles),:] = angles
        output_vert1[i,0:len(all_vert1_angles[i]),:] = all_vert1_angles[i]
        output_vert2[i,0:len(all_vert2_angles[i]),:] = all_vert2_angles[i]
        
    return [output_angles,output_vert1,output_vert2]
    
    
def plot_joint_angles(joint_angles):
    """
    Plot x,y,z translations and x,y,z rotations
    """
    fig = pylab.figure()
    labels = ['Med/Lat','Ant/Post','Sup/Inf','FE','LB','AR']
    for i in range(3):
        sp = fig.add_subplot(6,1,i+1)
        sp.plot(joint_angles[:,i])
        sp.set_ylabel(labels[i])
        sp.set_yticks(sp.get_ylim())
    for i in range(3,6):
        sp = fig.add_subplot(6,1,i+1)
        sp.plot(joint_angles[:,i]*180/np.pi)
        sp.set_ylabel(labels[i])
        sp.set_yticks(sp.get_ylim())
    pylab.show()

def save_joint_angles(filename,data):
    """
    Given a filename and an array of 2D data, this function will save the
    data as a text file with headers in the first row. 
    """
    data = np.asarray(data)
    n_frames = len(data)
    out_string = 'Euler Sequence: 213 [VTK]\n\t'
    out_string += ('\nFrame\tX-Trans\tY-Trans\tZ-Trans\tX-Rot(deg)\tY-Rot(deg)\tZ-Rot(deg)')
    for frame in range(n_frames):
        out_string += '\n%i'%frame
        out_string += ('\t%f\t%f\t%f\t%f\t%f\t%f' % 
                                  (data[frame,0],data[frame,1],data[frame,2],
                                   data[frame,3]*180/np.pi,data[frame,4]*180/np.pi,data[frame,5]*180/np.pi))
    fid = open(filename,'w')
    fid.write(out_string)
    fid.close()

def array_rms(data,max_frame=None):
    """
    This function will iterate over each dimension in the given array and 
    calculate the rms error for the frame column.
    
    data is a numpy array [Nx6], where each element is an array -> 
    [Frame,Dimension]
     
    max_frame [optional] is the last frame that should be considered when 
    calculating the RMS.
      
    output is numpy array[DIM] -> [Dimension] of rms errors
    """
    if max_frame == None:
        max_frame = len(data)
    #print "max_frame: ", max_frame
    # Create an array to store resutls 
    if len(data.shape)<2:
        n = len(data)
        rms_result = np.sqrt((data*data).sum()/len(data))
    else:
        n = data.shape[1]
        rms_result = np.zeros(data.shape[1])
        for i in range(n):
            # Iterate over each dimension and take a slice of frames
            col = data[:max_frame,i]
            #print "RMS -> Max Error: %f, Min Error: %f" % (col.max(),col.min())
            rms_result[i] = np.sqrt((col*col).sum()/len(col))
        
    return rms_result
    
def test_rms():
    """
    a little test for the rms function
    """
    # Create an array of [1x1x1x10x1]
    n = 1000
    n = int(raw_input("how many samples (more than 1000): "))
    a = np.ones((1,1,1,n,1))
    a[0,0,0,:,0]=np.sin(np.linspace(0,2*np.pi,n))
    b=array_rms(a)
    print "for %i values, b = %f" % (n,float(b))
    print "b should approach 1/sqrt(2)->[%f]" % float(1/np.sqrt(2))

def save_data(data,attrib,filename):
    """
    save data attribute to filename.
    """
    specimens=3
    trials=3
    bones=2
    out_shape = (specimen,)
    out_str = out_shape + '\n'
    fid = open(filename,'w')
    for i in range(specimens):
        for j in range(trials):
            for k in range(bones):
                out_shape = data[i].__getattribute__(attrib)[j][k].shape
                out_data = data[i].__getattribute__(attrib)[j][k].flatten()
                out_str += str(out_data)
                fid.write(out_str)
    fid.close()
    
    
def read_data(filename):
    """
    """
    fid = open(filename,'r')
    header_str = fid.readline()
    header = np.asarray(header_str.replace('(','').replace(')','').split(','),'i')
    data_str = fid.read()
    data = np.asarray(data_str.replace('[','').replace(']','').split(),'f')
    data.reshape(header)
    fid.close()
    return data



def show_landmarks(vol_path_list,marker_position_list,vol_poses=[],show_nums=False):
    """
    vol_list is a list of dictionaries, each with two keys:
        path - str path to a volume/model
        marker_list - list of 3D points (tuples/array/list)
    vol_poses is an optional list of poses for each volume in the path list.
        if no pose is given the volume is placed at the origin with (0,0,0) 
        rotations.
    eg. [{vol1_path, marker_list1},...,{voln_path, marker_listn}]
    
    Given a list of 3D points and a list of paths to volumes/models, 
    this method will create markers spheres at the 3D points and render the 
    result in a render window.
    """
    
    # Visualization Stuff
    renWin = vtk.vtkRenderWindow()
    ren = vtk.vtkRenderer()
    iren = vtk.vtkRenderWindowInteractor()
    style = vtk.vtkInteractorStyleTrackballCamera()
    iren.SetRenderWindow(renWin)
    iren.SetInteractorStyle(style)
    renWin.AddRenderer(ren)
    renWin.SetSize(512,512)
    
    # Read volume/model files
    for i,vol_path in enumerate(vol_path_list):
        ext = vol_path.lower().split('.')[1]
        if ext == 'stl':
            volume = JT.SurfaceModel()
            volume.LoadSTL(str(vol_path))
            ren.AddActor(volume)
        else:
            volume = JT.Volume2()
            volume.SetVolumeFileName(str(vol_path))
            ren.AddVolume(volume)
        try:
            volume.SetPose(vol_poses[i])
        except IndexError:
            print "No poses given, using (0,0,0,0,0,0)"
            pass
        # Place marker at volume center for visualization
        marker = JT.utils.create_sphere(3, (0, 33.0/255.0, 165.0/255.0))
        marker.SetPosition(volume.GetCenter())
        ren.AddActor(marker)
    #print "Added Volume at Position: %s" % str(volume)
    # Create marker spheres and connect them with lines         
    marker_list = []
    line_list = []
    

    #print "Image Data: %s" % str(volume._volImageData)
    axes = JT.AxesActor(5)
    axes.SetPosition((0,0,0))
    ren.AddActor(axes)
    for i,position in enumerate(marker_position_list):
        # UF Blue! (0, 33.0/255.0, 165.0/255.0)
        # UF Orange! (255.0/255.0, 74.0/255.0, 0)
        marker = JT.utils.create_sphere(3, (255.0/255.0, 74.0/255.0, 0))
        marker.SetPosition(position)
        print "Added Marker Position: %s" % str(position)
        marker_list.append(marker)
        ren.AddActor(marker)
        if show_nums:
            label = vtk.vtkTextActor3D()
            label.SetPosition(position[0]+2,position[1]+2,position[2]+2)
            props = label.GetTextProperty()
            props.SetFontSize(8)
            label.SetOrientation(90,-180,180)
            label.SetInput(str(i))
            ren.AddActor(label)
        
    for i in range(len(marker_list)):
        line = vtk.vtkLineSource()
        line.SetPoint1(marker_list[i].GetPosition())
        line.SetPoint2(marker_list[i-1].GetPosition())
        line_mapper = vtk.vtkPolyDataMapper()
        line_mapper.SetInput(line.GetOutput())
        line_actor = vtk.vtkActor()
        line_actor.SetMapper(line_mapper)
        line_list.append(line_actor)
        ren.AddActor(line_actor)

    renWin.Render()
    iren.Start()

    
def get_transform_from_markers(m1,m2,m3):
    """
    m1, m2, m3 are 3D marker positions.  This function will return
    a 4x4 numpy transformation matrix.  The position of the transformation
    is the center of the 3 marker positions. 
    """
    pass

def distance_between_markers(path,marker_list):
    """
    This function will calculate the distance between each marker pair in a 
    list and return the mean and standard deviation for each possible 
    marker pair in the list.
    
    inputs:
        path - path to a trc file with marker data
        marker_list = list of indices [1 based] that refer to markers in the trc file
        
    output:
        a dictionary, where keys are the marker pair (eg: '12') and the values
        are a tuple of (mean,standard deviation).
    """
    
    [n_frames, marker_data, marker_names, data_rate, headers] = marker_data_read(path)
    
    output = {}
    n_markers = len(marker_list)
    for i in range(n_markers):
        m1 = marker_list[i]
        for j in range(n_markers):
            if i == j: continue
            pair = '%i->%i' % (marker_list[i],marker_list[j])
            pair_rev = '%i->%i' % (marker_list[j],marker_list[i])
            if (pair not in output) and (pair_rev not in output):
                dist_vector = (marker_data[:,marker_list[i]+1,:] - 
                            marker_data[:,marker_list[j]+1,:])
                dist_scalar = np.sqrt(dist_vector[:,0]**2+
                                      dist_vector[:,1]**2+
                                      dist_vector[:,2]**2)
                result_mean = dist_scalar.mean()
                result_std = dist_scalar.std()
                result_rms = JT.utils.rms(dist_scalar)-result_mean
                output[pair] = np.asarray([result_mean, result_std, result_rms])
                print "%s - mean: %f, std: %f, rms: %f" % (pair,result_mean,result_std, result_rms)
#    print output
    return output
 


def build_local(P1, P2, P3,z_up=True):
    """
    % This function will read three Markers coordinates and 
    % output the origin, and direction cosine matrix that define the pose.
    % Input format should be a three points as column vectors
    % This function will take in three markers (P1, P2, P3), 
    % which define a plane.  
    %
    % For spine: P1 -> Left, P2 -> Right, P3 -> Anterior
    %
    % There are several different versions of this function depending on which
    % alignment is desired.
    %       BuildLocal:   The origin will be the midpoint of p1 and p2.
    %                     The X-axis will be in the direction from P1 to P2.
    %                     The Z-axis will be perpendicular to the 
    %                     plane, and the Y-axis will be orthogonal to the X- 
    %                     and Z-axes.
    %      z_up determines whether the z-axis should be up or down.
    %
    %
    % 10/14/2009 - translated from matlab to python
    """
    P1 = np.asarray(P1)
    P2 = np.asarray(P2)
    P3 = np.asarray(P3)
    
    O = (P1+P2)/2
    X = P2-P1
    X = X/JT.utils.mag(X)

    Yt = P3 - P1
    Yt = Yt/JT.utils.mag(Yt)
    Z = np.cross(X,Yt)
    Z = Z/JT.utils.mag(Z)
    Y = np.cross(Z,X)
    
    # Transpose to make unit vectors columns
    if z_up:
        R = np.asarray([X, Y, Z]).transpose()
    else:
        R = np.asarray([X, -Y, -Z]).transpose()
    
    T = np.eye(4)
    T[0:3,0:3] = R 
    T[0:3,3] = O
    
    return T   


def get_pose(transform,seq='123'):
    """
    AKA: get_pose_from_transform
   
    Given a 4x4 transform in numpy, return a 6 element vector
    (Tx,Ty,Tz,Rx,Ry,Rz).  
   
    For gold standard data use '123': Rotation order: T=Rz*Ry*Rx [x->y->z]
    For VTK actors, use '213': Rotation order: T=Rz*Rx*Ry [y->x->z]
    """
    # Get Translations out of transform
    transform = np.asarray(transform)
    Tx = transform[0][3]
    Ty = transform[1][3]
    Tz = transform[2][3]
   
    # Depending on sequence, get the rotations out.
    if seq == '123':
        Ry = np.arcsin(transform[2,0])    #'assumption' that cos(Ry)>0
        
        Rx_sin = np.arcsin(-transform[2][1]*1/np.cos(Ry))
        Rx_sin = np.arccos(transform[2][2]*1/np.cos(Ry))
        
        if (Rx_cos>np.pi/2 and Rx_sin>0): Rx=np.pi-Rx_sin
        if (Rx_cos>np.pi/2 and Rx_sin<0): Rx=-np.pi-Rx_sin 
        if (Rx_cos<=np.pi/2): Rx=Rx_sin
        
        Rz_sin = np.arcsin(-transform[1][0]*1/np.cos(Ry))
        Rz_cos = np.arccos(transform[0][0]*1/np.cos(Ry))
                
        if (Rz_cos>np.pi/2 and Rz_sin>0): Rz=np.pi-Rz_sin
        if (Rz_cos>np.pi/2 and Rz_sin<0): Rz=-np.pi-Rz_sin 
        if (Rz_cos<=np.pi/2): Rz=Rz_sin
       
       
    elif seq == '213':
        Rx = np.arcsin(-transform[2,1])    #'assumption' that cos(Rx)>0
        
        Ry_sin = np.arcsin(transform[2,0]*1/np.cos(Rx))
        Ry_cos = np.arccos(transform[2,2]*1/np.cos(Rx))
        
        if (Ry_cos>np.pi/2 and Ry_sin>0): Ry=np.pi-Ry_sin
        elif (Ry_cos>np.pi/2 and Ry_sin<0): Ry=-np.pi-Ry_sin 
        elif (Ry_cos<=np.pi/2): Ry=Ry_sin
        else:
            #print "DEBUG: 213-Ry"
            Ry=Ry_sin
        Rz_sin = np.arcsin(transform[0,1]*1/np.cos(Rx))
        Rz_cos = np.arccos(transform[1,1]*1/np.cos(Rx))
                
        if (Rz_cos>np.pi/2 and Rz_sin>0): Rz=np.pi-Rz_sin
        elif (Rz_cos>np.pi/2 and Rz_sin<0): Rz=-np.pi-Rz_sin 
        elif (Rz_cos<=np.pi/2): Rz=Rz_sin
        else:
            #print "DEBUG: 213-Rz"
            Rz=Rz_sin
    else:
        print "Sequence %s not recognized." % str(seq)
        return
       
    pose=np.array([Tx,Ty,Tz,Rx,Ry,Rz])
    return pose

def get_transform(pose,seq='123'):
    """
    AKA: get_transform_from_pose
   
    Given a 6 element vector (Tx,Ty,Tx,Rx,Ry,Rz) return a transformation
    matrix.  (NOT shifted or concatenated with anything else).
    Angles must be in radians
    Euler sequence 123 is used by Gold Standard Data, sequence 213 is
    used by VTK actors.
    """
    Tx,Ty,Tz,Rx,Ry,Rz = pose
    if seq == '123':
        # Tcs is TStartTransformation (body center to start pose)
        Tcs = np.eye(4)
        Tcs[0,0] = np.cos(Ry)*np.cos(Rz)
        Tcs[0,1] = -np.cos(Rx)*np.sin(Rz) + np.cos(Rz)*np.sin(Rx)*np.sin(Ry)
        Tcs[0,2] = np.sin(Rx)*np.sin(Rz) + np.cos(Rx)*np.cos(Rz)*np.sin(Ry)
        Tcs[0,3] = Tx
        Tcs[1,0] = np.cos(Ry)*np.sin(Rz)
        Tcs[1,1] = np.cos(Rx)*np.cos(Rz) + np.sin(Rx)*np.sin(Ry)*np.sin(Rz)
        Tcs[1,2] = -np.cos(Rz)*np.sin(Rx) + np.cos(Rx)*np.sin(Ry)*np.sin(Rz)
        Tcs[1,3] = Ty
        Tcs[2,0] = -np.sin(Ry)
        Tcs[2,1] = np.cos(Ry)*np.sin(Rx)
        Tcs[2,2] = np.cos(Rx)*np.cos(Ry)
        Tcs[2,3] = Tz
    elif seq == '312':
        # Gave Problems with testing marker positions
        Tcs = np.eye(4)
        Tcs[0,0] = cos(Ry)*cos(Rz) + sin(Rx)*sin(Ry)*sin(Rz)
        Tcs[0,1] = -cos(Ry)*sin(Rz) + cos(Rz)*sin(Rx)*sin(Ry)
        Tcs[0,2] = cos(Rx)*sin(Ry)
        Tcs[0,3] = Tx
        Tcs[1,0] = cos(Rx)*sin(Rz)
        Tcs[1,1] = cos(Rx)*cos(Rz)
        Tcs[1,2] = -sin(Rx)
        Tcs[1,3] = Ty
        Tcs[2,0] = -cos(Rz)*sin(Ry) + cos(Ry)*sin(Rx)*sin(Rz)
        Tcs[2,1] = sin(Ry)*sin(Rz) + cos(Ry)*cos(Rz)*sin(Rx)
        Tcs[2,2] = cos(Rx)*cos(Ry)
        Tcs[2,3] = Tz
    elif seq == '213':
        Tcs = np.eye(4)
        Tcs[0,0] = np.cos(Ry)*np.cos(Rz) - np.sin(Rx)*np.sin(Ry)*np.sin(Rz)
        Tcs[0,1] = -np.cos(Rx)*np.sin(Rz)
        Tcs[0,2] = np.cos(Rz)*np.sin(Ry) + np.cos(Ry)*np.sin(Rx)*np.sin(Rz)
        Tcs[0,3] = Tx
        Tcs[1,0] = np.cos(Ry)*np.sin(Rz) + np.cos(Rz)*np.sin(Rx)*np.sin(Ry)
        Tcs[1,1] = np.cos(Rx)*np.cos(Rz)
        Tcs[1,2] = np.sin(Ry)*np.sin(Rz) - np.cos(Ry)*np.cos(Rz)*np.sin(Rx)
        Tcs[1,3] = Ty
        Tcs[2,0] = -np.cos(Rx)*np.sin(Ry)
        Tcs[2,1] = np.sin(Rx)
        Tcs[2,2] = np.cos(Rx)*np.cos(Ry)
        Tcs[2,3] = Tz
   
    return Tcs



def get_transform2(pose,seq,frame='world'):
    """
    Return a 4x4 transformation matrix from the given pose and euler 
    sequence.
    
    Pose is [Tx,Ty,Tz,RotX,RotY,RotZ], rotations are in degrees.
    seq is a string of 3 characters (eg. 'xyz') which represent the order 
    of rotations with respect to the world coordinate system.  
    see: http://en.wikipedia.org/wiki/Euler_angles for details of 
    implementation.
    
    """
    rots = [0,0,0]
    for i,dim in enumerate(str(seq)):
        if (dim=='x' or dim=='1'):
            rots[i] = rotx
        if (dim=='y' or dim=='2'):
            rots[i] = roty
        if (dim=='z' or dim=='3'):
            rots[i] = rotz
      
    #print "Gold Standard (1->2->3 World)"
    #R = rz*ry*rx
    if frame=='world':
        T = np.dot(rots[2](pose[5]),np.dot(rots[1](pose[4]),rots[0](pose[3])))
    else:
        T = np.dot(rots[0](pose[3]),np.dot(rots[1](pose[4]),rots[2](pose[5])))
    T[0,3] = pose[0]
    T[1,3] = pose[1]
    T[2,3] = pose[2]
    #print R
    return T


def synchronize(data1,data2,plot=False):
    """
    Synchronize 1-D temporal data.  data2 will be shifted to the left and 
    at each iteration, the linear regression of data1 v data2 will be 
    calculated.  Then data1 will be shifted and the regressions calculated 
    for each iteration.  The best fitting regression will indicate when the 
    data is optimally synchronized.  A positive offset means that data1 was 
    fixed and data2 was shifted to match.  A negative offset means that
    data2 was fixed and data1 was shifted to match. 
    Returns: offset, r_max, direction of data shift [data1 or data2]
    """
    # For regression, use 2/3 of available data
    x1 = len(data1)
    x2 = len(data2)
    diff = x1 - x2
    if x1 < x2:
        print "ERROR. Data1 is shorter than Data2!"
        return
    
    data_tmp1 = np.zeros(x1+x2)
    data_tmp1[0:x1] = data1
     
    r_max = 0
    dir = 'data2'
    #print "DEBUG. Shape data1: %s, Shape data2: %s" % (str(data1.shape),str(data2.shape))
    for i in range(x2):
        data_tmp2 = np.zeros(x1+x2)
        data_tmp2[i:x2+i] = data2
        (slope,inter,r,p,stderr)=stats.linregress(data_tmp1,data_tmp2)
        #print('Linear regression using stats.linregress')
        #print('parameters: slope=%.2f inter=%.2f \nr=%.3f, p=%.3f, std error= %.3f' 
        #      % (slope,inter,r,p,stderr))
        if r>r_max:
            r_max=r
            i_best = i
            dir = 'data2'
    
    data_tmp2 = np.zeros(x1+x2)
    data_tmp2[0:x2] = data2
            
    for i in range(x2):
        data_tmp1 = np.zeros(x1+x2)
        data_tmp1[i:x1+i] = data1
        (slope,inter,r,p,stderr)=stats.linregress(data_tmp1,data_tmp2)
        #print('Linear regression using stats.linregress')
        #print('parameters: slope=%.2f inter=%.2f \nr=%.3f, p=%.3f, std error= %.3f' 
        #      % (slope,inter,r,p,stderr))
        if r>r_max:
            r_max=r
            i_best = i
            dir = 'data1'
            
    
    if plot:
        print "Plotting"
        print "Offset: %i, R: %f, dir: %s" % (i_best, r_max, dir)
        fig=pylab.figure()
        ax=fig.add_subplot(111)
        if dir=='data1':
            ax.plot(data1)
            ax.plot(data2[i_best:])
        else:
            ax.plot(data1[abs(i_best):])
            ax.plot(data2)
        pylab.ion()
        pylab.show()
    
    #print "Best offset is: %f, with R: %f" % (i_best,r_max)
    if dir == 'data1':
        # if data1 was shifted, the offset will be returned as a negative
        i_best = -i_best
        
    return [i_best,r_max]

def synchronize2(data1,data2,plot=False):
    """
    Another method to synchorize periodic data by matching peak and valley
    """
    peak1 = pylab.find(data1==np.max(data1))[0]
    peak2 = pylab.find(data2==np.max(data2))[0]
    valley1 = pylab.find(data1==np.min(data1))[0]
    valley2 = pylab.find(data2==np.min(data2))[0]
    off_peak = peak2-peak1
    off_vall = valley2-valley1
    #off_mean = np.sum([off_peak,off_vall])/2
    print ("peak offset: %f, valley offset: %f" 
           % (off_peak,off_vall))
    
    if plot:
        print "Plotting"
        fig=pylab.figure()
        ax=fig.add_subplot(111)
        if off_peak > 0:
            ax.plot(data1)
            ax.plot(data2[off_peak:])
        else:
            ax.plot(data1[abs(off_peak):])
            ax.plot(data2)
        pylab.ion()
        pylab.show()
        
    return off_peak
    

def test_sync():
    """
    test synchronization code.
    """
    x = np.linspace(0,2*np.pi,100)
    y = np.sin(x)
    y2 = np.sin(x+np.pi/8)
    [i_best,stderr_min,y_new,y_new2] = synchronize(y, y2)
    
    pylab.plot(x[:-i_best],y[i_best:],'r-',x[:-i_best],y2[:-i_best],'bx')
    pylab.show()

def rotx(ang):
    """
    rotation of 'ang' about the X axis
    """
    return np.array([[1,        0,      0,  0],
             [0,   np.cos(ang), np.sin(ang),  0],
             [0,  -np.sin(ang), np.cos(ang),  0],
             [0,        0,      0,   1]])

def roty(ang):
    """
    rotation of 'ang' about the Y axis
    """
    return np.array([[np.cos(ang),  0, -np.sin(ang),  0],
             [0,        1,      0,  0],
             [np.sin(ang),  0,  np.cos(ang),  0],
             [0,        0,      0,   1]])

def rotz(ang):
    """
    rotation of 'ang' about the Z axis
    """
    return np.array([[np.cos(ang),    np.sin(ang), 0, 0],
             [-np.sin(ang),   np.cos(ang), 0, 0],
             [0     ,         0, 1, 0],
             [0     ,         0, 0, 1]])

def rotx_sym(ang):
    """
    rotation of 'ang' about the X axis
    """
    return sympy.Matrix([[1,        0,      0,  0],
             [0,   sympy.cos(ang), sympy.sin(ang),  0],
             [0,  -sympy.sin(ang), sympy.cos(ang),  0],
             [0,        0,      0,   1]])

def roty_sym(ang):
    """
    rotation of 'ang' about the Y axis
    """
    return sympy.Matrix([[sympy.cos(ang),  0, -sympy.sin(ang),  0],
             [0,        1,      0,  0],
             [sympy.sin(ang),  0,  sympy.cos(ang),  0],
             [0,        0,      0,   1]])

def rotz_sym(ang):
    """
    rotation of 'ang' about the Z axis
    """
    return sympy.Matrix([[sympy.cos(ang),    sympy.sin(ang), 0, 0],
             [-sympy.sin(ang),   sympy.cos(ang), 0, 0],
             [0     ,         0, 1, 0],
             [0     ,         0, 0, 1]])

def test_rot_seq(seq='xyz',frame='world'):
    """
    Create a rotation matrix of the sequence Rx->Ry->Rz.

    Create symbolic rotation matrices about the X, Y, and Z axes.
    Useful for visualizing the effect of different rotation sequences.
    """
    seq_list=['xyz','xzy','yxz','yzx','zyx','zxy',
              'xzx','xyx','yxy','yzy','zyz','zxz']
    
    frame = 'local'
    
    x=sympy.Symbol('Rx')
    y=sympy.Symbol('Ry')
    z=sympy.Symbol('Rz')
    
    rots = [0,0,0]
    for seq in seq_list:
        for i,dim in enumerate(seq):
            if (dim=='x' or dim=='1'):
                rots[i] = rotx_sym
            if (dim=='y' or dim=='2'):
                rots[i] = roty_sym
            if (dim=='z' or dim=='3'):
                rots[i] = rotz_sym
          
        #print "Gold Standard (1->2->3 World)"
        #R = rz*ry*rx
        if frame=='world':
            R = rots[2](z)*rots[1](y)*rots[0](x)
        else:
            R = rots[0](x)*rots[1](y)*rots[2](z)
        print '-*-'*36
        print seq
        print R
        #return R
        
#    print "VTK (2->1->3 World)"
#    R = rz*rx*ry
#    print R
#    # VTK docs say they do this, but my results are not good 
    #R = ry*rx*rz

    #print R

def test_arcsin():
    print -np.arcsin(np.pi/4)

def test_spine_kinematics():
    plot_results_from_project_file(r'C:\Users\bryan\phd\data\Spine1_8\Spine1_8_all_goodInitial.xml')
    #plot_results_from_project_file(r'C:\Users\bryan\phd\data\Spine1_1\Spine1_1_C4-C5.xml')
    
    
def test_jt_anatomic_joint_angles():
    config_file = r'C:\Users\bryan\phd\data\Spine2\Spine2_config.pickle'
    save_name = r'C:\Users\bryan\phd\data\Spine2\Spine2_2_output.dat'
    jt_anatomic_joint_angles(config_file,save_name,plot=True)
#    jt_anatomic_joint_angles(jt_project = sp2_5)
#    jt_anatomic_joint_angles(jt_project = sp2_9)

def test_mocap_analysis():
    trc2_2 = r'Spine2_2c.trc'
    trc2_5 = r'Spine2_5.trc'
    trc2_9 = r'Spine2_9.trc'
    
    sp2_2 = r'C:\Users\bryan\phd\data\Spine2\Spine2_C6C7_markers.xml'
    sp2_5 = r'C:\Users\bryan\phd\data\Spine2\Spine2_5\Spine2_5_C6-C7.xml'
    sp2_9 = r'C:\Users\bryan\phd\data\Spine2\Spine2_9\Spine2_9_C6-C7.xml'
    
#    mocap_analysis(sp2_2,trc2_2)
    mocap_analysis(sp2_2,trc2_2)
#    mocap_analysis(sp2_2,trc2_9)    

def test_distance_between_markers():
    trc_path = r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine1_1.trc'
    marker_list = [9,7,8]
    distance_between_markers(trc_path, marker_list)
    marker_list = [11,12,10]
    distance_between_markers(trc_path, marker_list)
    trc_path = r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine2_2.trc'
    marker_list = [14,15,13]
    distance_between_markers(trc_path, marker_list)
    marker_list = [17,18,16]
    distance_between_markers(trc_path, marker_list)
    trc_path = r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine3_1.trc'
    marker_list = [5,6,4]
    distance_between_markers(trc_path, marker_list)
    marker_list = [8,9,7]
    

if __name__ == '__main__':
#    test_rot_seq()
    test_sync()
#    test_jt_anatomic_joint_angles()
#    test_mocap_analysis()
#    test_distance_between_markers()
#    test_arcsin()